import os
import sys

sys.path.append(".")

import math
import numpy as np
# import converter
import matplotlib
matplotlib.use('TkAgg')
from matplotlib import pyplot as plt
from matplotlib.lines import Line2D
from mpl_toolkits.mplot3d import Axes3D
from mpl_toolkits.mplot3d.art3d import Line3D
import time_processor as tp
import geo_processor as gp
from scipy import interpolate

def read_tum_data(tum_path):

    result = []

    num = 0
    with open(tum_path) as f:
        for line in f:
            if line[0] != '#':
                pass
            num = num + 1 
            items = line.split(' ')
    
            result1 = []
            result1.append(float(items[0])) # timestamp
            result1.append(float(items[1])) # data1
            result1.append(float(items[2])) # data2
            result1.append(float(items[3])) # data3
            result1.append(float(items[4])) # data4
            result1.append(float(items[5])) # data5
            result1.append(float(items[6])) # data6
            result1.append(float(items[7])) # data7

            result.append(result1)

    return np.array(result)

def read_image_timestamp(image_timestamp_path):

    result = []

    num = 0
    with open(image_timestamp_path) as f:
        for line in f:
            if line[0] != '#':
                pass
            num = num + 1 
            result.append(int(line)) # timestamp
    
    return result

def change_timestamp_and_save(tum_result, image_timestamp, save_path):
    output_tum = open(save_path, "w+")
    
    for i in range(tum_result.shape[0]):
        index = int(tum_result[i,0] / 0.1 + 0.5)
        if index >= len(image_timestamp):
            break
        output_tum.write("{:.0f}".format(image_timestamp[index]))
        output_tum.write(" ")
        output_tum.write("{:.12f}".format(tum_result[i,1]))
        output_tum.write(" ")
        output_tum.write("{:.12f}".format(tum_result[i,2]))
        output_tum.write(" ")
        output_tum.write("{:.12f}".format(tum_result[i,3]))
        output_tum.write(" ")
        output_tum.write("{:.12f}".format(tum_result[i,4]))
        output_tum.write(" ")
        output_tum.write("{:.12f}".format(tum_result[i,5]))
        output_tum.write(" ")
        output_tum.write("{:.12f}".format(tum_result[i,6]))
        output_tum.write(" ")
        output_tum.write("{:.12f}".format(tum_result[i,7]))
        output_tum.write("\n")

    output_tum.close()

if __name__ == "__main__":
    path1 = r"/shared/VIO/openvslam-main/build/result/trajectory_0_19300.txt"
    path2 = r"/media/cjg/Elements/pano20210902/pro2/data/image_timestamp.txt"
    save_path = r"/shared/VIO/openvslam-main/build/result/trajectory_0_19300_new.txt"
    tum_result = read_tum_data(path1)
    image_timestamp = read_image_timestamp(path2)
    change_timestamp_and_save(tum_result, image_timestamp, save_path)

